#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/GetMap.h>
#include <tf/transform_listener.h>

#define EPS 0.000001


class RosWrapper {

	typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

public:
	RosWrapper(double ti, double ri) :
		ac("move_base", true), seqNumber(0), translationIncrement(ti), rotationIncrement(ri) {
		while(!ac.waitForServer(ros::Duration(1.0))){
			ROS_INFO("Waiting for the move_base action server to come up");
		}
		ros::service::waitForService("static_map", 10000);

		discretePose = getCurrentPoseInMap(); //just so that it's populated
	}

	bool setHomePosition() {
		discretePose = getCurrentPoseInMap();
		return true;
	}

	bool moveFoward() {
		move_base_msgs::MoveBaseGoal goal;

		goal.target_pose.pose = discretePose.pose;

		goal.target_pose.header.seq = seqNumber++;
		goal.target_pose.header.frame_id = "/map";

		double yaw = tf::getYaw(discretePose.pose.orientation);

		double x = goal.target_pose.pose.position.x;
		double y = goal.target_pose.pose.position.y;

		goal.target_pose.pose.position.x = x + (translationIncrement * cos(yaw));
		goal.target_pose.pose.position.y = y + (translationIncrement * sin(yaw));

		ROS_INFO("%f %f (%f) -> %f %f", x, y, (yaw/(2*M_PI))*360, goal.target_pose.pose.position.x, goal.target_pose.pose.position.y);

		ac.sendGoal(goal);
		ac.waitForResult();

		bool success = (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
		if(success) {
			discretePose.pose = goal.target_pose.pose;
		}

		return success;
	}

	bool rotate(int direction) {
		int unitDirection = direction / abs(direction);

		move_base_msgs::MoveBaseGoal goal;

		goal.target_pose.header.seq = seqNumber++;
		goal.target_pose.header.frame_id = "/map";

		double yaw = tf::getYaw(discretePose.pose.orientation);
		double newYaw = yaw + rotationIncrement * unitDirection;

		goal.target_pose.pose = discretePose.pose;
		goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(newYaw);

		ROS_INFO("%f -> %f", (yaw/(2*M_PI))*360, (newYaw/(2*M_PI))*360);

		ac.sendGoal(goal);
		ac.waitForResult();

		bool success = (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
		if(success) {
			discretePose.pose = goal.target_pose.pose;
		}

		return success;
	}

private:


	bool doubleEqual(double a, double b) const {
		return fabs(a - b) <= EPS;
	}


	geometry_msgs::PoseStamped getCurrentPoseInMap() {

		geometry_msgs::PoseStamped baseLinkPose = getStampedPose("/base_link", 0, 0, 0, 0);
		geometry_msgs::PoseStamped mapPose = getStampedPose("/map", 0, 0, 0, 0);

		std::string error;
		try {
			while(!tfListener.waitForTransform(baseLinkPose.header.frame_id,
						   mapPose.header.frame_id,
						   baseLinkPose.header.stamp,
						   ros::Duration(1.0),
						   ros::Duration(0.01),
						   &error)) {
				baseLinkPose.header.stamp = ros::Time::now();
				ROS_INFO("Waiting for transform between map and base_link: %s", error.c_str());
			}

			tfListener.transformPose(mapPose.header.frame_id, baseLinkPose, mapPose);
		}
		catch (tf::TransformException ex) {
			ROS_ERROR("%s", ex.what());
			exit(1);
		}
		return mapPose;
	}

	geometry_msgs::PoseStamped getStampedPose(std::string frame, double x, double y,
											double z, double orientation) {
		geometry_msgs::PoseStamped pose;

		pose.header.frame_id = frame;
		pose.header.stamp = ros::Time::now();

		pose.pose.position.x = x;
		pose.pose.position.y = y;
		pose.pose.position.z = z;

		pose.pose.orientation = tf::createQuaternionMsgFromYaw(orientation);

		return pose;
	}

	ros::NodeHandle n;
	tf::TransformListener tfListener;
	MoveBaseClient ac;
	geometry_msgs::PoseStamped discretePose;
	unsigned int seqNumber;
	double translationIncrement;
	double rotationIncrement;
};
